package graph;

import geocoding.ALGOTrip;
import geocoding.DistanceCalculator;

import java.util.Enumeration;
import java.util.LinkedList;
import java.util.Vector;

public class StudentSpecialGraph extends SearchableGraph {
	 /**
     * implementation of the AStar algorithm for computing routes in the
     * bus network
     */
    
	private ALGOPlacemarkNode currentNode;
	private ALGOPlacemarkNode targetNode;
	private double [] bestCostSoFarInNode;
	private double targetDist;
	public static double gw = 1, 
						 hw = 1,
						 c = 0;
	
	public static long stepCount = 0;
	
	public StudentSpecialGraph() {
		super();
	}

	public void initSearch(ALGONode startNode, ALGONode targetNode) {
		agenda = new ALGOHeap(nodes.size());
		bestCostSoFarInNode = new double[nodes.size()];
		ALGOEdge [] succ = this.getEdgesForNode(startNode.getNodeID());
		
		for (int i = 0; i < nodes.size(); i++) {
    		ALGOPlacemarkNode p = (ALGOPlacemarkNode)nodes.get(i);
    		
    		p.removeFromOpenList();
    		p.removeFromClosedList();
    		bestCostSoFarInNode[i] = Double.MAX_VALUE;
    	}
		
		for (int i = 0; i < succ.length; i++) {
			bestCostSoFarInNode[succ[i].getSink()] =
				gw * succ[i].getCosts() +
				hw * ((ALGOPlacemarkNode)nodes.get(succ[i].getSink())).costs((ALGOPlacemarkNode)targetNode) + 
				c;
			agenda.insert(new AStarHeapData(new ALGOPath(succ[i]),
					                        bestCostSoFarInNode[succ[i].getSink()],
					                        succ[i].getCosts()));
			nodes.get(succ[i].getSink()).putOnOpenList(agenda);
		}
		
		for (int i = 0; i < nodes.size(); i++) {
    		LinkedList<ALGOEdge> l = edges.get(new Integer(i));
    		if (l != null) {
    			for(int j = 0; j < l.size(); j++) {
    				ALGOEdge e = l.get(j);
    				e.setOptimal(false);
    			}
    		}
		}
		
		
		currentNode = (ALGOPlacemarkNode)startNode;
		this.targetNode = (ALGOPlacemarkNode)targetNode;
		
		targetDist = ((ALGOPlacemarkNode)startNode).costs((ALGOPlacemarkNode)targetNode);
	}
	
	public boolean solutionFound() {
		if(currentNode.getNodeID() == targetNode.getNodeID())
			System.out.println("Sum NextStep = " + stepCount);
			
		return (currentNode.getNodeID() == targetNode.getNodeID());
	}
	
	public boolean noSolution() {
		return agenda.isEmpty();
	}
	
	public static double calculateAngleBetweenNodes(ALGOPlacemarkNode now, ALGOPlacemarkNode target, ALGOPlacemarkNode next) {
		double angle;
		double[] lat = new double[3];
		double[] lng = new double[3];
    	final double r = 6378;
    	
    	lat[0] = now.getValue().getPoint().getNumericalCoordinates()[0];
    	lng[0] = now.getValue().getPoint().getNumericalCoordinates()[1];
    	lat[1] = target.getValue().getPoint().getNumericalCoordinates()[0];
    	lng[1] = target.getValue().getPoint().getNumericalCoordinates()[1];
    	lat[2] = next.getValue().getPoint().getNumericalCoordinates()[0];
    	lng[2] = next.getValue().getPoint().getNumericalCoordinates()[1];
    	
    	// Umrechnung in kartesische Koordianten
    	double[][] p = new double[3][3]; // [punkt][coord]; coord: 0=z, 1=x, 2=y
    	
    	for(int i = 0; i < 3; i++)
    	{
    		p[i][1] = r*Math.cos(DistanceCalculator.deg2rad(lat[i]))*Math.cos(DistanceCalculator.deg2rad(lng[i]));
    		p[i][2] = r*Math.cos(DistanceCalculator.deg2rad(lat[i]))*Math.sin(DistanceCalculator.deg2rad(lng[i]));
        	p[i][0] = r*Math.sin(DistanceCalculator.deg2rad(lat[i]));
    	}
    	
    	/*for(int i = 0; i < 3; i++)
    		for(int j = 0; j < 3; j++)
    			p[i][j] = i==j ? 1 : 0;*/
    	
    	// Schenkel-Vektoren
    	double[] v1 = new double[3];
    	for(int i = 0; i < 3; i++)
    		v1[i] = p[1][i] - p[0][i];
    	
    	double[] v2 = new double[3];
    	for(int i = 0; i < 3; i++)
    		v2[i] = p[2][i] - p[0][i];
    	   	
    	// Normen
    	double abs_v1 = Math.sqrt(Math.pow(v1[0], 2) + Math.pow(v1[1], 2) + Math.pow(v1[2], 2));
    	double abs_v2 = Math.sqrt(Math.pow(v2[0], 2) + Math.pow(v2[1], 2) + Math.pow(v2[2], 2));
    	
    	// Skalarprodukt
    	double vskalar = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
    	
//    	System.out.println("FOO v1=(" + v1[0] + "," + v1[1] + "," + v1[2] + "), v2=(" + v2[0] + "," + v2[1] + "," + v2[2] + ")");
//		System.out.println("now = " + now.getValue().getName() + ", target = " + target.getValue().getName() + ", next = " +next.getValue().getName());
    	
    	if (vskalar == 0){
    		angle = 0;
    	}
    	else{
    	
    	//Winkel am Punkt p1, Schenkel p2, p3
    	angle = DistanceCalculator.rad2deg(Math.acos(vskalar / (abs_v1 * abs_v2)));
    	}
        
        return angle;
	}
	
	
	public void nextStep() {	
	 	AStarHeapData hd;
    	ALGOPlacemarkNode min;
    	double currentG;
    	double currentF;
    	
    	stepCount++;
    	
    	hd = (AStarHeapData) agenda.retrieve();
    	agenda.deleteHighest();
    	
    	min = (ALGOPlacemarkNode)getNode(hd.getData().lastElement().getSink());
    		
    	if (min.getNodeID() == targetNode.getNodeID()) {
    		currentNode = min;
    		currentSolution = hd.getData();
    		System.out.println("Ziel gefunden");
    		return;
    	}

    	min.putOnClosedList();
    	ALGOEdge [] minEdges = this.getEdgesForNode(min.getNodeID());   		
    	for (int i = 0; i < minEdges.length; i++) {
    		ALGOEdge e = minEdges[i];
    		ALGOPlacemarkNode succ = (ALGOPlacemarkNode)nodes.get(e.getSink());
    		
    		double angle = calculateAngleBetweenNodes(min, targetNode, succ);
    		System.out.println("Winkel " + angle);
    		
    		if(angle > 180) // angle nun zwischen -180 und +180
    			angle = angle - 360;
    		
    		angle = Math.abs(angle); // angle zwischen 0 (gut) und 180 (sau schlecht)
    		
    		// in e-Weise Route vergiften
    		if(angle > 150)
    			c = 2000;
    		else if(angle > 100)
    			c = 750;
    		else if(angle > 75)
    			c = 450;
    		else if(angle > 40)
    			c = 50;
    		else if(angle > 20)
    			c = 1;
    		else if(angle > 10)
    			c = 1;
    		
    		// Entfernung der beiden Knoten
    		double[] c1 = min.getValue().getPoint().getNumericalCoordinates();
    		double[] c2 = succ.getValue().getPoint().getNumericalCoordinates();
    		double diff = DistanceCalculator.getDistance(c1, c2);
    		
    		if(diff < 10)
    		{
    			diff = 10 - diff;
    			if(diff < 1)
    				diff = 1;
    			
    			c /= diff;
    		}
    		else 
    		  c *= diff;
    		
//    		String foo = min.getValue().getName() + " nach "+ succ.getValue().getName();
//    		System.out.println(foo+": c = "+c+" :: diff = " + diff);
    		
    		// Elipse
    		double cc;
    		System.out.println("XX:" + succ.costs(targetNode) + " :: " + targetDist);
    		if(succ.costs(targetNode) > targetDist  * 0.5)
        	  cc = 5;
    		else if(succ.costs(targetNode) > targetDist  * 0.2)
      		  cc = 1;
    		else if(succ.costs(targetNode) > targetDist  * 0.1)
              cc = 0.1;
    		else 
    	      cc = 0.01;
    		
    		c *= cc;
    		
    		double[] current_node_coords = min.getValue().getPoint().getNumericalCoordinates();
    		
    		

    		currentG = hd.getActualCosts() + e.getCosts() + c;
    		c = 0;
    		currentF = gw * currentG + 
    				   hw * succ.costs(targetNode) +
    				   c;

    		if (!succ.isOpen() && !succ.isClosed()) {
    			// noch nicht besucht und auch bisher noch kein Kandidat dafür
    			// succ wird Kandidat für weitere Suche
    			
    			succ.putOnOpenList(agenda);
    			ALGOPath p1 = hd.getData().clone();
    			p1.add(minEdges[i]);
    			agenda.insert(new AStarHeapData(p1, currentF, currentG));
    			
    			bestCostSoFarInNode[succ.getNodeID()] = currentF;
    		}
    		else if (currentF < bestCostSoFarInNode[succ.getNodeID()]) {
    			// succ schon besucht oder bereits Kandidat und besserer Weg zu succ als bisher
    			// dann speichere diesen Pfad

    			bestCostSoFarInNode[succ.getNodeID()] = currentF;
    			e.highlight();

    			if (succ.isClosed()) {
    				System.out.println("remove from closed: " + succ.getValue().getName());
    				// succ sogar schon besucht, nicht nur Kandidat
    				// dann prüfe noch mal alles ab succ (backtracking)
    				
    				ALGOPath p1 = hd.getData().clone();
        			p1.add(minEdges[i]);
        			agenda.insert(new AStarHeapData(p1, currentF, currentG));
    				succ.removeFromClosedList();
    				succ.putOnOpenList(agenda);
    			}
    		}
    	}
    }
	
	public ALGOTrip computeBestPath(ALGOPlacemarkNode startNode, ALGOPlacemarkNode targetNode) {
		return computeShortestPath(startNode, targetNode);
	}
}
